function [a,b,c] = inv_kin(x,y,z)
%addpath ('C:\Program Files\MATLAB\R2014a\toolbox\rvctools');
startup_rvc

%define the time between end effector poses

%t = 0:0.05:2;
t = 0:2;


%----------------------------------------------------------

%   Denavit-Hartenberg parameters that describe the arm

%----------------------------------------------------------

%theta is the variable for this joint. the value provided here
%simply serves as a place holder but theta's value will
%be handled by the link object.




theta = [0, 0, 0];
d = [0.07, 0, 0];
a = [.04, .16, 0];
alpha = [-pi/2, 0, 0];

%Sigma defines wether the joint is revolute (0) or prismatic (1) 
sigma = [0, 0, 0];

%define all links above as link objects
for i = 1:3
   
    L(i) = Link([theta(i), d(i), a(i), alpha(i)]);

end

%serial link all the links

robot_arm = SerialLink(L, 'name', 'Lego Bot');

%---Shift the first and last joint around to increase accuracy of model

%shift the origin of the robot to the playing field
%robot_arm.base = transl(0, 0, .05);
%shift the tool up by 17.7cm for the pen
robot_arm.tool = transl(0.193, 0, 0);

%find the pose of the end effector
  
%T = robot_arm.fkine(zero);
T1 = [ 1     0    0     .135;  %out radially
      0     1    0     .135;   %left right
      0     0    1      -.065; %up down
      0     0    0     1.0];
  
%T = robot_arm.fkine(zero);
T2 = [ 1     0    0     x;  %out radially
      0     1    0     y;   %left right
      0     0    1    z; %up down
      0     0    0     1.0];

%----------------------------------------------------------

%   Cartesian Motion

%----------------------------------------------------------

Ts = ctraj(T1, T2, length(t));

% figure;
% subplot(2,1,1);
% plot(t, transl(Ts));
% legend('show');
% title('Translation');
% 
% subplot(2,1,2);
% plot(t, tr2rpy(Ts));
% legend('show');
% title('Orientation');

%----------------------------------------------------------

%   Inverse Kinematics: given a poses (Ts) return the joint space
%   solutions needed to move the end effector through Ts

%----------------------------------------------------------

%for underactuated robots (fewer that 6DOF) need to define a
%mask vector since the solution space has more dimensions that the
%manipulator
mask = [1 1 1 0 0 0]; %3DOF

%choosing initial position 0 0 2 seems to set the elbow pointing
%the right way.
qs = robot_arm.ikine(Ts,[0 0 2], mask, 'alpha', .85, 'ilimit', 800);


%display(qs);
%{
for i = 1:length(qs)
   robot_arm.plot(qs(i,1:3))
   pause(.1);
end
%}    
qs1 = qs * 180 / pi;
a=qs1(3,1);
b=qs1(3,2);
c=qs1(3,3);
display(a);
display(b);
display(c);
end